/*
 *  perpix.h
 *  class_perpixClassify
 *
 *  Created by Kris Kitani on 5/16/12.
 *
 *  Revised by Cheng Li on 7/12/12
 *
 *  Copyright 2012 CMU. All rights reserved.
 *
 */

#include <iostream>
#include <fstream>
#include <iomanip>
#include <opencv2/opencv.hpp>
#include <opencv2/flann/config.h>
#include <opencv2/flann/flann.hpp>
#include <opencv2/legacy/legacy.hpp>		// EM
#include <opencv2/contrib/contrib.hpp>		// colormap
#include <opencv2/nonfree/nonfree.hpp>		// SIFT

#include "LC_HEX.h"

using namespace std;
using namespace cv;

#define LC_ROOT_STRING "C:/Users/LC1332/Desktop/CMU work/Ki's code/_FGS/"

class perpix
{

public:
	
	bool FEAT_RGB;
	bool FEAT_HSV;
	bool FEAT_LAB;
	bool FEAT_BRIEF;
	bool FEAT_ORB;
	bool FEAT_SIFT;
	bool FEAT_LOC;
	
	int WIN_SIZE;
	int TREE_DEPTH;
	int NUM_STACK;
	int VISUALIZE;
	bool SAVE_HAND_DB;
	bool SAVE_AVI;
	
	bool TRAIN_ON;
	bool TEST_ON;
	bool COMPUTE_METRICS;
	bool COMPUTE_CONTOUR;
	
	bool ADDITIVE;
	bool LIGHT_COMP;
	bool VERBOSE;
	int SAMPLE_STEP;		// step size for subsampling
	int NEG_STEP;			// additional step size for subsampling negative examples
	
	string METHOD;
	
	vector <CvDTree> dtree;
	vector<CvBoost> adaboost;
	vector<CvGBTrees> gbtree;
	
	//EM::EM em[2];
	//Cheng Li
	EM em[2];
	
	float p_pos;
	float p_neg;
	
	CvSVM svm;
	
	string model_code;
	
	string train_basename;
	string test_basename;
	string root;
	string msktyp;
	string ext;
	
	
	//Cheng Li
	int unsupervise_kernel_number;
	
	
	//Cheng Li
	bool HEX_USE;
	
	float initFeatureScale;
	int   featureScaleLevels;
	float featureScaleMul;
	int train_initXyStep;
	int test_initXyStep;
	int initImgBound;	
	float scale;					// scale image size
	
	//Cheng Li
	perpix(){;}

	//Cheng Li
	perpix(int argc, char * const argv[]);
	
	
	
	void init(string method);
	void set_train(string root, string msktyp, string train_basename, string ext, int f_start, int f_end, int f_rate);
	void run_train();
	
	int f_end; 
	int f_rate;
	int f_start;
	int train_start;						// level to start training
	
	//Cheng Li
	int addition_id;
	
	void set_test(string root, string msktyp, string test_basename, string ext, int f_start, int f_end, int f_rate);
	void run_test();
	
private:
	
	void colormap(Mat _src, Mat &dst);
	void colormap(Mat src, Mat &dst, int do_norm);
	Mat  getBinMask(Mat mask);
	void appendOrbDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc);
	void appendBriefDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc);
	void appendColorDescriptors(Mat src,vector<KeyPoint> keypts,Mat &desc, string space);
	void appendSiftDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc);
	void appendFeatures(Mat src,vector<KeyPoint> &keypts,Mat &desc);
	void appendLocationDescriptors(Mat src,vector<KeyPoint> keypts,Mat &desc);
	
	Mat  getLabels(vector<KeyPoint> keypts,Mat im);
	Mat  appendStackedResponse(Size sz, vector<KeyPoint> &keypts, Mat &desc, string method, int level);
	
	void ConvexityDefects( const Mat& points, const vector<int>& hull, vector<CvConvexityDefect> &defects );
	void trainModel(Mat &data, Mat &lab, string model_filename, string method);
	Mat  computeSingleLayerResponse(Size sz, vector<KeyPoint> keypts, Mat &desc, string method, int level);
	
	void removeZeroRegions(Mat response, vector<KeyPoint> &keypts, Mat &desc);
	
	string getModelString();
	Mat appendAverageResponse(Mat response, Size sz, vector<KeyPoint> keypts, Mat &desc);
	
	Mat loadGT(int f, string basename);
	void loadModels(int k);
	
	void storeResponse(vector<KeyPoint> keypts,Mat response,Mat label, Mat &Px_c);
	
	Mat appendDiffStackedResponse(Size sz, vector<KeyPoint> &keypts, Mat &desc, string method, int level, Mat &fx);
	
	void subsampleDescriptors(vector<KeyPoint> &keypts, Mat &desc,int step, int offset);
	void subsampleDescriptors(vector<KeyPoint> &keypts, Mat &desc, int sample_step, int neg_step, int offset, Mat &f_x, Mat &lb);
		
	Mat computeSingleLayerResponse(Size sz, vector<KeyPoint> keypts, Mat &desc, string method, int level, Mat &f_x);
	Mat appendAverageDiffResponse(Mat response, Size sz, vector<KeyPoint> keypts, Mat &desc);
	float sigmoid_factor;
	
	void postprocessing(Mat src, Mat response);
	Mat feat;
	Mat wristxy;
	flann::Index flannIndex;
	int db;
	
	//Cheng Li
	void computeColorHist_HSV(Mat src, Mat &hist);
	
};

void perpix::computeColorHist_HSV(Mat src, Mat &hist)
{
   
	int bins[] = {4,4,4};
    if(src.channels()!=3) exit(1);
    Mat tmp;
    src.copyTo(tmp);
    Mat hsv;
    cvtColor(src,hsv,CV_BGR2HSV_FULL);
    int histSize[] = {bins[0], bins[1], bins[2]};
    Mat his;
    his.create(3, histSize, CV_32F);
    his = Scalar(0);   
    CV_Assert(hsv.type() == CV_8UC3);
    MatConstIterator_<Vec3b> it = hsv.begin<Vec3b>();
    MatConstIterator_<Vec3b> it_end = hsv.end<Vec3b>();
    for( ; it != it_end; ++it )
    {
        const Vec3b& pix = *it;
        his.at<float>(pix[0]*bins[0]/256, pix[1]*bins[1]/256,
pix[2]*bins[2]/256) += 1.f;
    }
       
    // ==== Remove small values ==== //
    float minProb = 0.01;
    minProb *= hsv.rows*hsv.cols;
    Mat plane;
    const Mat *_his = &his;
   
    NAryMatIterator itt = NAryMatIterator(&_his, &plane, 1);   
    threshold(itt.planes[0], itt.planes[0], minProb, 0, THRESH_TOZERO);
    double s = sum(itt.planes[0])[0];

    // ==== Normalize ==== //
    s = 1./s * 255.;
    itt.planes[0] *= s;
   
    itt.planes[0].copyTo(hist);

}

void perpix::init(string method)
{
	
	METHOD = method;		// dt,gbt,ada,em
	
	// ==== VISUALIZATION FLAGS ==== //
	VISUALIZE  = 1;				
	VERBOSE = 1;
	SAVE_AVI = 1;
	// ============================= //
	
	//Cheng Li
	HEX_USE = true;
	
	
	
	
	WIN_SIZE   = 5;		// * 3,5,7		
	// ================== //
	
	
	TEST_ON = 1;
	TRAIN_ON = 1;
	
	COMPUTE_METRICS = 1; // precision recall, 0-1 Loss
	COMPUTE_CONTOUR = 1;
	
	ADDITIVE = 1;
	sigmoid_factor = 5;
	NUM_STACK  = 8;		// 6,8
	LIGHT_COMP = 0;
	
	SAMPLE_STEP = 2; // 2 normally,
	NEG_STEP = 10;
	
	if(METHOD=="em")
	{
		ADDITIVE = false;
		NUM_STACK = 1;
		SAMPLE_STEP = 16;
	}
	else if(METHOD=="dt")
	{
		TREE_DEPTH = 12;	// 6-16 decision tree parameter
		dtree = vector<CvDTree>(NUM_STACK);
	}
	else if(METHOD=="ada")
	{
		ADDITIVE = false;		// Classification only: Cannot be used as additive model
		NUM_STACK = 1;
		adaboost = vector<CvBoost>(NUM_STACK);
		SAMPLE_STEP = 5;		// 50
	}
	else if(METHOD=="gbt"){
		NUM_STACK = 1;
		gbtree = vector<CvGBTrees>(NUM_STACK);
		SAMPLE_STEP = 50;
	}
	
	scale = 0.50;		// 0.5 ETI(1.0) FPV(0.5)
	
	model_code = getModelString();
	
	initFeatureScale = 1.f;		// inital size
	featureScaleLevels = 1;		// one level
	featureScaleMul = 1.00f;	// multiplier (ignored if only one level)
	
	initImgBound = floor(WIN_SIZE*0.5);
	//if(!FEAT_ORB && !FEAT_BRIEF) initImgBound = WIN_SIZE;
	if(FEAT_BRIEF) initImgBound = 28;
	if(FEAT_ORB) initImgBound = 31;
	if(FEAT_SIFT) initImgBound = initImgBound;
}





void perpix::run_test()
{
	
	if(!TEST_ON) return;
	
	cout << "Run Test" << endl;
	VideoWriter avi;
	Mat frm, src;
	
	
	float sum_loss = 0;
	float sum_pos = 0;
	float sum_tp = 0;
	float sum_gt_pos = 0;
	
	if(METHOD=="em"){
		string filename = root+"models/"+train_basename+model_code+"_prior.xml";
		ifstream fs(filename.c_str());
		fs >> p_pos;
		fs >> p_neg;
		cout << "p_pos: " << p_pos << endl;
		cout << "p_neg: " << p_neg << endl;
	}
	
	ofstream fs;
	if(COMPUTE_METRICS)
	{
		string filename = root + "output/loss/" + test_basename + model_code + "_test_loss.txt";
		fs.open(filename.c_str());
		fs << "frame\tloss\tprecision\trecall\tfmeasure\n";
	}
	
	ofstream fcc;
	ofstream fgt;
	ofstream fbb;
	ofstream fin;
	
	if(COMPUTE_CONTOUR)
	{
		string filename = root + "output/contours/" + test_basename+model_code+"_test_contours.txt";
		fcc.open(filename.c_str());
		
		if(COMPUTE_METRICS){
			filename = root + "output/contours/" + test_basename+model_code+"_gt_contours.txt";
			fgt.open(filename.c_str());
		}
		
		filename = root + "output/bbox/" + test_basename+model_code+"_test_bbox.txt";
		fbb.open(filename.c_str());
		
		filename = root + msktyp +test_basename + "/" + test_basename + "_cont_ob.txt";
		fin.open(filename.c_str());
	}
	
	vector<int> q;
	q.push_back(CV_IMWRITE_JPEG_QUALITY);
	q.push_back(100);
	
	DenseFeatureDetector dfd;
	dfd = DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul,test_initXyStep,initImgBound,true,false);
	
	loadModels(NUM_STACK);
	
	string vidfile = root+"vid/"+test_basename+ext;
	cout << "Opening video: " << vidfile << endl;
	VideoCapture cap(vidfile);
	
	int f = -1;	
	
	//Cheng Li
	LC_HEX my_lc_hex;
	
	if(HEX_USE)
	{
		my_lc_hex.init(root,METHOD, & model_code,unsupervise_kernel_number);
	
		my_lc_hex.init_test();
	}
	
	
	
	while(waitKey(1)!=27)
	{
		cap >> frm;
		if(!frm.data) break;
		f++;
		if(VERBOSE) cout << "======== " << f << " =========" << endl;
		
		//if((f%f_rate)!=0) continue;
		//Cheng Li
		if(((f-f_start)%f_rate)!=0) continue;
		
		
		if(f<f_start) continue;
		if(f>f_end) break;
		
		resize(frm,src,Size(),scale,scale);
		imshow("src",src);
		
		//Cheng Li
		Mat hist_feature;
		computeColorHist_HSV(src,hist_feature);
		
		// ==== illumination feature ==== //
		if(LIGHT_COMP){
			Mat gry;
			cvtColor(src,gry,CV_BGR2GRAY);
			double maxVal = 255.*0.95;
			//minMaxLoc(gry,NULL,&maxVal,&Point(),&Point());
			//cout << "maxVal:" << maxVal << endl;
			//maxVal *= 0.95; 
			threshold(gry,gry,maxVal,255,THRESH_BINARY);
			Mat top5;
			src.copyTo(top5,gry);
			imshow("top5",top5);
			Scalar s = mean(top5,gry);
			//float percent = countNonZero(gry)*1.0/(gry.rows*gry.cols);
			//cout << "non-zero: " << countNonZero(gry) << "/" << percent << endl;
			//cout << "avg:" << Mat(s) << endl;
			//if(percent<0.001) s *= 1.0;
			//Mat gfeat = Mat(s).t();
			//if(!gif.data) gfeat.copyTo(gif);
			//else gif.push_back(gfeat);
			//cout << gif << endl;
			if(countNonZero(gry)>100)
			{
				vector<Mat> mv;
				split(src,mv);
				mv[0] = mv[0] / s(0) * 255.;
				mv[1] = mv[1] / s(1) * 255.;
				mv[2] = mv[2] / s(2) * 255.;
				merge(mv,src);
			}
			imshow("color normed",src);
		}
		// ============================== //
		
		
		vector<KeyPoint> keypts;
		Mat desc;
		dfd.detect(src,keypts);
		
		appendFeatures(src,keypts,desc);
		
		//Cheng Li
		Mat hex_result;

		if(HEX_USE)
		{						
			my_lc_hex.predict_frame(hist_feature,desc,hex_result);			
		}
		
		
		

		Mat sum_res;
		Mat fx;

		if(!HEX_USE)
		{
			if(ADDITIVE) sum_res = appendDiffStackedResponse(src.size(),keypts,desc,METHOD,NUM_STACK-1,fx);
			else appendStackedResponse(src.size(),keypts,desc,METHOD,NUM_STACK-1);
		}

		

		//CHENG Li
		Mat response;
		if(HEX_USE) response=my_lc_hex.MoveResponsetoimage(src.size(), keypts, hex_result);
		else  response=computeSingleLayerResponse(src.size(),keypts,desc,METHOD,NUM_STACK-1); // need layer index here...				 

		
		
		
		Mat p_x;
		Mat exp_fx;
		
		
		//Cheng Li in windows should be
		if(ADDITIVE  && 0){
		//if(ADDITIVE){
			Mat cum_res = sum_res + response;	// add responses
			exp(sigmoid_factor*cum_res,exp_fx);
			p_x = exp_fx / (1+exp_fx);
			response = p_x;
		}
		
		Mat im;
		if(COMPUTE_METRICS){
			im = loadGT(f,test_basename);
			if(!im.data){cout << "Cannot load gt image." << endl; exit(1);}
			resize(im,im,src.size(),0,0, INTER_NEAREST);
			imshow("gt",im);
		}

		
		

		Mat bin;
		vector<vector<Point> > co;
		vector<Vec4i> hi;
		Rect r;
		
		if(COMPUTE_METRICS)
		{
			threshold(im,bin,250,255,THRESH_BINARY);
			findContours(bin,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);
			if(co.size()>0)
			{	
				for(int i=0;i<co.size();i++)
				{
					if(contourArea(Mat(co[i])) < 100) continue;
					fgt << f << " ";
					fgt << contourArea(Mat(co[i])) << " ";
					for(int j=0;j<(int)co[i].size();j++){
						fgt << co[i][j].x << " ";
						fgt << co[i][j].y << " ";
					}
					fgt << endl;
				}
			}
		}
		
		Mat res;
		medianBlur(response,res,5);
		Mat dsp;
		colormap(res,dsp,0);
		imshow("res",dsp);

		
		
		threshold(res,bin,0.50,255,THRESH_BINARY);
		bin.convertTo(bin,CV_8UC1);
		imshow("bin",bin);
		findContours(bin,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
		Mat contours = bin*0;
		Mat contours_filled = bin*0;
		
		if(co.size()>0)
		{	
			for(int i=0;i<co.size();i++)
			{
				if(contourArea(Mat(co[i])) < 100) continue;
				r = boundingRect(Mat(co[i]));
				
				fin << f;
				fin << " " << -1;			// unknown part label
				fin << " " << r.x;
				fin << " " << r.y;
				fin << " " << r.width;
				fin << " " << r.height;
				fin << " " << -1;			// unknown wrist pos_x
				fin << " " << -1;			// unknown wrist pos_y
				fin << endl;
				
				fbb << f << " ";
				fbb << r.x << " ";
				fbb << r.y << " ";
				fbb << r.width << " ";
				fbb << r.height << " ";
				fbb << endl;
				
				fcc << f << " ";
				fcc << contourArea(Mat(co[i])) << " ";
				for(int j=0;j<(int)co[i].size();j++){
					fcc << co[i][j].x << " ";
					fcc << co[i][j].y << " ";
				}
				fcc << endl;
				
				drawContours(contours, co,i,Scalar::all(255),5,CV_AA);
				drawContours(contours_filled,co,i,Scalar::all(255),-1,CV_AA);
			}
		
			imshow("contour",contours);
			stringstream ss;
			ss << root + msktyp + test_basename +"/"+ test_basename + "_contob_" << f << ".jpg";

			imwrite(ss.str(),contours,q);
		} 
		
		
		
		if(VISUALIZE)
		{
			//cout << "Visualize" << endl;
			
			Mat dsp;
			if(ADDITIVE){
				colormap(response,dsp,0);
				imshow("dsp",dsp);
			}
			else colormap(response,dsp);
			
			stringstream ss;
			ss.str("");
			ss << "Final response " << NUM_STACK - 1 << endl;
			imshow(ss.str(),dsp);
			
			
			cvtColor(contours_filled,dsp,CV_GRAY2BGR);
			addWeighted(dsp,0.5,src,0.5,0, dsp);
			imshow("alpha",dsp);
			
			//vector<int> q;
			//q.push_back(CV_IMWRITE_JPEG_QUALITY);
			//q.push_back(100);
			//imwrite("src.jpg",src,q);
			//imwrite("dsp.jpg",dsp,q);
			//cout << "avi" << endl;
			
			if(SAVE_AVI){
				if(!avi.isOpened()){
					string vidout = root+"vid/"+test_basename+model_code+".avi";
					cout << "Creating video:" << vidout << endl;
					avi.open(vidout,CV_FOURCC('F','L','V','1'),30,dsp.size(), true);
				}
				else avi << dsp;	
			}
		}
		
		
		
	
		if(COMPUTE_METRICS)
		{
			cout << " === Compute Test Metrics ===" << endl;
			Mat Px_c;									// response - class
			Mat lb = getLabels(keypts,im);
			storeResponse(keypts,response,lb,Px_c);
		
			//Cheng Li
			Mat re;
			if(HEX_USE) re = hex_result.t();//
			else re = Px_c.t().row(0);	// response
			
			Mat la = Px_c.t().row(1);	// label
			
			//Cheng Li
			Px_c.release();
			
			//Cheng Li
			if(HEX_USE) my_lc_hex.compare_label(re,la);
			
			
			Mat pos;
			threshold(re,pos,0.49,1.0,THRESH_BINARY);
			Mat tp;
			Mat mask;
			la.convertTo(mask,CV_8UC1);
			pos.copyTo(tp,mask);
			
			Mat zeroOneLoss = pos-la;
			
			cout << " All positive detections: " << sum(pos)(0) << endl;
			cout << " True positive detections: " << sum(tp)(0) << endl;
			cout << " All true positives: " << sum(la)(0) << "/" << la.cols << endl;
			
			float loss = sum(abs(zeroOneLoss))(0);
			cout << " Zero-one loss:" << loss << endl;
			float precision = sum(tp)(0) / (sum(pos)(0)+FLT_MIN);
			cout << " Precision:" << precision << endl;
			float recall = sum(tp)(0) / (sum(la)(0)+FLT_MIN);
			cout << " Recall:" << recall << endl;
			float fmeasure = 2.0*precision*recall / (precision+recall);
			cout << " Fmeasure:" << fmeasure << endl;
			
			sum_loss += loss;
			sum_pos += sum(pos)(0);
			sum_tp += sum(tp)(0);
			sum_gt_pos += sum(la)(0);
			
			fs << f << "\t";
			fs.unsetf(ios::floatfield); 
			fs << loss << "\t";		// zero one loss
			fs.precision(10);
			fs.setf(ios::fixed,ios::floatfield);
			fs << precision << "\t";		// precision
			fs << recall << "\t";			// recall
			fs << fmeasure << "\t";
			fs << endl;
			
		}
		else if(HEX_USE)
		{
			cout << " === Compute Test Metrics ===" << endl;
			Mat Px_c;									// response - class
			Mat lb = getLabels(keypts,im);
			storeResponse(keypts,response,lb,Px_c);
		
			//Cheng Li
			Mat re;
			if(HEX_USE) re = hex_result.t();//
			else re = Px_c.t().row(0);	// response
			
			Mat la = Px_c.t().row(1);	// label
			
			//Cheng Li
			Px_c.release();
			
			//Cheng Li
			if(HEX_USE) my_lc_hex.compare_label(re,la);
		}				
	
	}
	
	
	if(HEX_USE) my_lc_hex.OutputResult();
	 
	if(COMPUTE_METRICS)
	{
		float precision = sum_tp / sum_pos;
		float recall = sum_tp / sum_gt_pos;
		float fmeasure = 2.0*precision*recall / (precision+recall);
		
		fs.unsetf(ios::floatfield); 
		fs << "TOTAL RATES" << endl;
		fs << "sum_tpo:\t" << sum_tp << endl;
		fs << "sum_pos:\t" << sum_pos << endl;
		fs << "sum_gt_pos:\t" << sum_gt_pos << endl;
		
		fs << "loss\tprecision\trecall\tfmeasure\n";
		fs.unsetf(ios::floatfield); 
		fs << sum_loss << "\t";
		fs.precision(10);
		fs.setf(ios::fixed,ios::floatfield);
		fs << precision << "\t";			// precision
		fs << recall << "\t";		// recall
		fs << fmeasure << "\t";
		fs << endl;
	}
	
	//fs << Px_c << endl;
}
	
void perpix::run_train()
{	
	//if(!TRAIN_ON) return;
	//if(ADDITIVE) run_train_additive();
	//else run_train_aggregate();
	
	if(!TRAIN_ON) return;
	
	if(ADDITIVE) cout << "Training additive model" << endl;
	else cout << "Training aggregate model" << endl;
		
	string filename = root+"output/loss/"+train_basename+model_code+"_train_loss.txt";
	ofstream fileout(filename.c_str());
	fileout << "Layer\tloss\tprecision\trecall\tfmeasure\n";
	
	Mat frm, src;
	VideoCapture cap;
	
	//if(!FEAT_ORB && !FEAT_BRIEF) initImgBound = WIN_SIZE;
	//else if(FEAT_BRIEF) initImgBound = 28;
	
	DenseFeatureDetector dfd;
	dfd = DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul,train_initXyStep,initImgBound,true,false);
	
	//Cheng Li
	LC_HEX my_lc_hex;	
	if(HEX_USE) my_lc_hex.init(root,METHOD,& model_code,unsupervise_kernel_number);
	
	for(int k=train_start;k<NUM_STACK;k++)
	{
		cout << "k=" << k << endl;
		loadModels(k);
		
		Mat dats;
		Mat labs;
		Mat f_x;				// gradient
				
		cap.open(root+"vid/"+train_basename+ext);
		int f = -1;
		
		while(waitKey(1)!=27)
		{
			
			cap >> frm;
			if(!frm.data) break;
			f++;
			
			//if( ((f%f_rate)!=0) || (f<f_start) ) continue;
			//Cheng Li
			if( (((f-f_start)%f_rate)!=0) || (f<f_start) ) continue;
			
			if(f>f_end) break;
			
			resize(frm,src,Size(),scale,scale);
			imshow("src",src);
			
			Mat hist_feature;

			//Cheng Li
			if(HEX_USE)
			{				
				computeColorHist_HSV(src,hist_feature);
			}
			
			
			// ==== illumination feature ==== //
			if(LIGHT_COMP){
				Mat gry;
				cvtColor(src,gry,CV_BGR2GRAY);
				double maxVal = 255.*0.95;
				threshold(gry,gry,maxVal,255,THRESH_BINARY);
				Mat top5;
				src.copyTo(top5,gry);
				imshow("top5",top5);
				Scalar s = mean(top5,gry);
				
				if(countNonZero(gry)>100)
				{
					vector<Mat> mv;
					split(src,mv);
					mv[0] = mv[0] / s(0) * 255.;
					mv[1] = mv[1] / s(1) * 255.;
					mv[2] = mv[2] / s(2) * 255.;
					merge(mv,src);
				}
				imshow("color normed",src);
			}
			// ============================== //
			
			Mat im = loadGT(f,train_basename);
			resize(im,im,src.size(),0,0,INTER_NEAREST);
			imshow("im",im);
			
			
			if(VISUALIZE){
				Mat dsp;
				src.copyTo(dsp,im);
				imshow("ground truth",dsp);
			}
			
			vector<KeyPoint> keypts;
			Mat desc;
			
			dfd.detect(src,keypts);
			
			appendFeatures(src,keypts,desc);
			
			Mat fx;
			
			if(ADDITIVE) appendDiffStackedResponse(src.size(),keypts,desc,METHOD,k,fx);
			else fx = appendStackedResponse(src.size(),keypts,desc,METHOD,k);
			
			Mat lb = getLabels(keypts,im);
			
			subsampleDescriptors(keypts,desc,NUM_STACK*SAMPLE_STEP,NEG_STEP,k,fx,lb);						// every layer trained on different pixels			
			
			//Cheng Li
			if(HEX_USE) my_lc_hex.add_frame(hist_feature,desc,lb);
			
			if(!dats.data) desc.copyTo(dats);
			else dats.push_back(desc);
			if(!labs.data) lb.copyTo(labs);
			else labs.push_back(lb);
			
			if(!f_x.data) fx.copyTo(f_x);
			else f_x.push_back(fx);
			
			cout << f << "] data:" << dats.rows << " skin:" << countNonZero(labs) << " dims:" << dats.cols << endl;
			
		} // while loop (video)
		
		//==== kmeans ==== //
//		if(0){
//			gif.convertTo(gif,CV_32FC1,1/255.,0);
//			cout << gif << endl;
//			
//			int K = 3;
//			Mat kmeans_labs;
//			int attempts = 10;
//			Mat centers;
//			TermCriteria crit(TermCriteria::MAX_ITER +TermCriteria::EPS,30,0.01);
//			kmeans(gif,K,kmeans_labs,crit,attempts,KMEANS_PP_CENTERS,centers);
//			cout << "kmeans labs:" << kmeans_labs.t() << endl;
//			cout << "kmeans centers:" << centers << endl;
//		}
		//================ //
		
		cout << "Data dims: " << dats.cols << endl;
		if(VISUALIZE) destroyAllWindows();		
		

		if(HEX_USE)
		{
			my_lc_hex.work();
			break;
		}
		else
		{
		
			Mat p_x;
			Mat exp_fx;
			Mat res;
			
			if(ADDITIVE){
				exp(sigmoid_factor*f_x,exp_fx);
				p_x = exp_fx / (1 + exp_fx);		// initially all 0.5
				res = labs - p_x;
			}
			else{
				if(k==0) f_x += 0.5;
				res = labs;
			}
			
			stringstream modelfile;
			modelfile << root << "models/" << train_basename << model_code << "_" << k;
			trainModel(dats,res,modelfile.str(),METHOD);
			
			
			//========================================================//
			if(!ADDITIVE) p_x = f_x;
			
			cout << "p_x:" << p_x.rows << "," << p_x.cols << endl;
			cout << "labs:" << labs.rows << "," << labs.cols << endl;
			
			Mat bin;
			threshold(p_x,bin,0.49,1.0,THRESH_BINARY);
			Mat zeroOneLoss = labs - bin;
			
			Mat pos;
			bin.convertTo(pos,CV_8UC1);			// all positive responses
			Mat tp;
			labs.copyTo(tp,pos);				// all true positives in positive responses		
			
			cout << "detected positives:" <<  sum(pos)(0) << endl;
			cout << "detected true positives:" <<  sum(tp)(0) << endl;
			cout << "all true positives:" <<  sum(labs)(0) << endl;
			
			float precision = sum(tp)(0) / sum(pos)(0);
			float recall = sum(tp)(0) / sum(labs)(0);
			float fmeasure = 2.0*precision*recall / (precision+recall);
			
			cout << "==========================" << endl;
			cout << "Zero One Loss: " << sum(abs(zeroOneLoss))(0) << endl;
			cout << "Precision: " << precision << endl;
			cout << "Recall: " << recall << endl;
			cout << "Fmeasure: " << fmeasure << endl;
			
			fileout << k;	// layer
			fileout << "\t" << sum(abs(zeroOneLoss))(0);
			fileout << "\t" << precision;
			fileout << "\t" << recall;
			fileout << "\t" << fmeasure;
			fileout << endl;
			
			if(METHOD=="em"){
				p_pos = sum(labs)(0) / labs.rows;
				p_neg = sum(1-labs)(0) / labs.rows;
				cout << "p_pos: " << p_pos << endl;
				cout << "p_neg: " << p_neg << endl;
				string filename = root+"models/"+train_basename+model_code+"_prior.xml";
				ofstream fs(filename.c_str());
				fs << p_pos << endl;
				fs << p_neg << endl;
			}
		}
					
	} // for each layer ...
}


void perpix::set_train(string root, string msktyp, string train_basename, string ext, int f_start, int f_end, int f_rate)
{	
	this->root = root;
	this->train_basename = train_basename;
	this->ext = ext;
	this->msktyp = msktyp;
	this->f_start = f_start;
	this->f_end = f_end;
	this->f_rate = f_rate;
	train_initXyStep = 1;		// space between pixels for training (must be 1)
	train_start = 0;			// level to start training
}


void perpix::set_test(string root, string msktyp, string test_basename, string ext, int f_start, int f_end, int f_rate)
{	
	this->root = root;
	this->test_basename = test_basename;
	this->ext = ext;
	this->msktyp = msktyp;
	this->f_start = f_start;
	this->f_end = f_end;
	this->f_rate = f_rate;	
	test_initXyStep = 1;		// test time grid spacing (must be 1 if layers > 1)
}


void perpix::storeResponse(vector<KeyPoint> keypts,Mat response, Mat label, Mat &Px_c){
	
	//cout << "Store response" << endl;
	//cout << "keypt:" << keypts.size() << " labs:" << label.rows << endl;
	//cout << "response type:" << response.type() << "=?=" << CV_32FC1 << endl;
	
	Px_c = Mat::zeros(keypts.size(),2,CV_32FC1);
	
	for(int k=0;k<(int)keypts.size();k++)
	{
		int _r = round(keypts[k].pt.y);			// center point
		int _c = round(keypts[k].pt.x);
		Px_c.at<float>(k,0) = response.at<float>(_r,_c);
		Px_c.at<float>(k,1) = label.at<float>(k,0);
		if(Px_c.at<float>(k,1)!=1.0 && Px_c.at<float>(k,1)!=0.0){
			cout << k << ") non-binary response:" << label.at<float>(k,0) << endl; 
		}
	}

}


void perpix::loadModels(int k){
	
	for(int i=0;i<k;i++)
	{
		stringstream modelfile;

		if(METHOD=="ada"){
			modelfile << root << "models/" << train_basename << model_code << "_" << i << ".yml";
			cout << "Opening trained model: " << modelfile.str() << endl;
			adaboost[i].load(modelfile.str().c_str());
		}
		else if(METHOD=="dt"){
			modelfile << root << "models/" << train_basename << model_code << "_" << i << ".yml";
			cout << "Opening trained model: " << modelfile.str() << endl;
			dtree[i].load(modelfile.str().c_str());
		}
		else if(METHOD=="em"){
			
			FileStorage fs;
			FileNode fileNode;
			
			modelfile.str("");
			modelfile << root << "models/" << train_basename << model_code << "_" << i << "_pos.yml";
			cout << "Opening trained model: " << modelfile.str() << endl;
			
			fs = FileStorage(modelfile.str().c_str(), FileStorage::READ);
			fileNode = fs["EM"];
			em[0].read(fileNode);
			
			
			modelfile.str("");
			modelfile << root << "models/" << train_basename << model_code << "_" << i << "_neg.yml";
			cout << "Opening trained model: " << modelfile.str() << endl;
			
			fs = FileStorage(modelfile.str().c_str(), FileStorage::READ);
			fileNode = fs["EM"];
			em[1].read(fileNode);
			
		}
	}
}
void perpix::appendFeatures(Mat src,vector<KeyPoint> &keypts,Mat &desc)
{	
	int dims = 0;
	if(FEAT_ORB) dims += 32;
	if(FEAT_BRIEF) dims += 16;
	if(FEAT_SIFT) dims += 128;
	if(FEAT_RGB) dims += 3*WIN_SIZE*WIN_SIZE;
	if(FEAT_HSV) dims += 3*WIN_SIZE*WIN_SIZE;
	if(FEAT_LAB) dims += 3*WIN_SIZE*WIN_SIZE;
	if(FEAT_LOC) dims += 1;
	
	double t = getTickCount();
	desc = Mat::zeros((int)keypts.size(),dims,CV_32FC1);
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " allocate memory:" << t << " secs." << endl;
	
	t = getTickCount();
	int d = 0;
	int nd = (int)keypts.size();
	
	//cout << "Keypoints:" << (int)keypts.size() << endl;
	//cout << "Descriptor size:" << desc.rows << " " <<desc.cols << endl;
	
	if(FEAT_ORB){
		Mat _desc = desc(Rect(d,0,32,nd));
		appendOrbDescriptor(src,keypts,_desc);
		d+=32;
	}
	if(FEAT_BRIEF){
		Mat _desc = desc(Rect(d,0,16,nd));
		appendBriefDescriptor(src,keypts,_desc);
		d+=16;
	}
	if(FEAT_SIFT){
		Mat _desc = desc(Rect(d,0,128,nd));
		appendSiftDescriptor(src,keypts,_desc);
		d += 128;
	}
	
	if((int)keypts.size() != desc.rows){
		cout << "Keypoints:" << (int)keypts.size() << endl;
		cout << "Descriptor size:" << desc.rows << " " <<desc.cols << endl;
		cout << "ERROR: Number of features is incorrect! Bad allocation of memory!" << endl;
		exit(1);
	}
	
	if(FEAT_RGB){
		Mat _desc = desc(Rect(d,0,3*WIN_SIZE*WIN_SIZE,nd));
		appendColorDescriptors(src,keypts,_desc,"RGB");
		d += (3*WIN_SIZE*WIN_SIZE);
	}
	if(FEAT_HSV){
		Mat _desc = desc(Rect(d,0,3*WIN_SIZE*WIN_SIZE,nd));
		appendColorDescriptors(src,keypts,_desc,"HSV");
		d += (3*WIN_SIZE*WIN_SIZE);
	}

	if(FEAT_LAB){
		Mat _desc = desc(Rect(d,0,3*WIN_SIZE*WIN_SIZE,nd));
		appendColorDescriptors(src,keypts,_desc,"LAB");
		d += (3*WIN_SIZE*WIN_SIZE);
	}
	
	if(FEAT_LOC)
	{
		Mat _desc = desc(Rect(d,0,1,nd));
		appendLocationDescriptors(src,keypts,_desc);
		d += 1;
	}
	t = (getTickCount()-t)/getTickFrequency();
	
	cout << " compute all features:" << t << " secs." << endl;
	
	//cout << "dims: " << desc.cols << endl;
}

void perpix::colormap(Mat src, Mat &dst)
{
	colormap(src, dst,1); // normalize
}

void perpix::colormap(Mat src, Mat &dst, int do_norm)
{
	
	double minVal,maxVal;
	minMaxLoc(src,&minVal,&maxVal,NULL,NULL);
	//cout << "colormap minmax: " << minVal << " " << maxVal << endl;
	Mat im = src;
	if(do_norm) im = (src-minVal)/(maxVal-minVal);		// normalization [0 to 1]
	
	
	if(1){														// my HSV mapping with masking
		
		Mat mask;	
		mask = Mat::ones(src.size(),CV_8UC1)*255.0;	
		
		compare(im,FLT_MIN,mask,CMP_GT);						// one color values greater than X	
		//threshold(src,src,FLT_MIN,0,THRESH_TOZERO);			// 		
		
		Mat U8;
		im.convertTo(U8,CV_8UC1,255,0);
		
		Mat I3[3],hsv;
		I3[0] = U8 * 0.85;
		I3[1] = mask;
		I3[2] = mask;
		merge(I3,3,hsv);
		cvtColor(hsv,dst,CV_HSV2RGB_FULL);
	}
	else{
		/*
		 COLORMAP_AUTUMN = 0,		// works
		 COLORMAP_BONE = 1,			// works
		 COLORMAP_JET = 2,
		 COLORMAP_WINTER = 3,
		 COLORMAP_RAINBOW = 4,		// not implemented
		 COLORMAP_OCEAN = 5,
		 COLORMAP_SUMMER = 6,
		 COLORMAP_SPRING = 7,
		 COLORMAP_COOL = 8,
		 COLORMAP_HSV = 9,			// broken
		 COLORMAP_PINK = 10,
		 COLORMAP_HOT = 11,
		 COLORMAP_MKPJ1 = 12,		// works
		 COLORMAP_MKPJ2 = 13
		 */
		Mat U8;
		im.convertTo(U8,CV_8UC1,255,0);
		applyColorMap(U8,dst,13);
	}
	
}

Mat perpix::getBinMask(Mat mask){
	Mat binmask(mask.size(),CV_8UC1);
	binmask = mask & GC_FGD;
	return binmask;
}

void perpix::appendOrbDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc){
	
	double t = getTickCount();
	Mat orb_desc;
	OrbDescriptorExtractor ode;
	ode.compute(src,keypts,orb_desc);
	orb_desc.convertTo(orb_desc,CV_32FC1);
	
	//cout << "orb_desc:" << orb_desc.cols << endl;
	
	for(int i=0;i<(int)orb_desc.rows;i++){
		Mat row = orb_desc.row(i);
		normalize(row,row,1.0,0,NORM_L1);
	}
	
	orb_desc.copyTo(desc);
	
//	if(!desc.data) orb_desc.copyTo(desc);
//	else{
//		desc = desc.t();
//		orb_desc = orb_desc.t();
//		desc.push_back(orb_desc);
//		desc = desc.t();
//	}
	
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " compute ORB features:" << t << " secs." << endl;
}

void perpix::appendBriefDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc){
	
	double t = getTickCount();
	Mat brief_desc;
	BriefDescriptorExtractor bde(16);
	bde.compute(src,keypts,brief_desc);
	brief_desc.convertTo(brief_desc,CV_32FC1);
	
	for(int i=0;i<(int)brief_desc.rows;i++){
		Mat row = brief_desc.row(i);
		normalize(row,row,1.0,0,NORM_L1);
	}
	
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " compute BRIEF features:" << t << " secs." << endl;
	
	t = getTickCount();
	
	if(1){
		brief_desc.copyTo(desc);
	}
	else{
		if(!desc.data) brief_desc.copyTo(desc);
		else{
			desc = desc.t();
			brief_desc = brief_desc.t();
			desc.push_back(brief_desc);
			desc = desc.t();
		}
	}
	
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " copy BRIEF features:" << t << " secs." << endl;
	
}

void perpix::appendSiftDescriptor(Mat src,vector<KeyPoint> &keypts,Mat &desc)
{	
	double t = getTickCount();
	Ptr<DescriptorExtractor> siftde = DescriptorExtractor::create("SIFT");
	Mat sift_desc;
	siftde->compute(src,keypts,sift_desc);	
	sift_desc.convertTo(sift_desc,CV_32FC1);
	for(int i=0;i<(int)sift_desc.rows;i++){
		Mat row = sift_desc.row(i);
		normalize(row,row,1.0,0,NORM_L1);
	}
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " compute SIFT features:" << t << " secs." << endl;
	
	t = getTickCount();
	sift_desc.copyTo(desc);
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " copy SIFT features:" << t << " secs." << endl;
}

void perpix::appendLocationDescriptors(Mat src,vector<KeyPoint> keypts,Mat &desc)
{
	// location from center
	double t = getTickCount();
	//Mat loc_desc = Mat::zeros((int)keypts.size(),1,CV_32FC1);
	
	float cx = src.cols*0.5;
	float cy = src.rows*0.5;
	
	for(int k=0;k<(int)keypts.size();k++)
	{
		int _r = round(keypts[k].pt.y);
		int _c = round(keypts[k].pt.x);
		float dist = hypot((_r-cy)/src.rows, (_c-cx)/src.cols);
		desc.at<float>(k,0) = dist;
		//cout << loc_desc.at<float>(k,0) << endl; 
	}
	
//	if(!desc.data) loc_desc.copyTo(desc);
//	else{
//		desc = desc.t();
//		loc_desc = loc_desc.t();
//		desc.push_back(loc_desc);
//		desc = desc.t();
//	}
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " Compute Location features:" << t << " secs." << endl;
	
	
}

void perpix::appendColorDescriptors(Mat src,vector<KeyPoint> keypts,Mat &desc, string space){
	
	double t = getTickCount();
	
	int code;
	if(space=="RGB") code = CV_BGR2RGB;
	else if(space=="HSV") code = CV_RGB2HSV_FULL;
	else if(space=="LAB") code = CV_BGR2Lab;
	
	Mat color;
	cvtColor(src,color,code);
	t = (getTickCount()-t)/getTickFrequency();
	if(VERBOSE) cout << " compute color features: " << t << " secs." << endl;
	
	vector<float> vf(WIN_SIZE*WIN_SIZE*3,0);
	
	t = getTickCount();
	int a;
	for(int k=0;k<(int)keypts.size();k++)
	{
		int r = round(keypts[k].pt.y) - floor(WIN_SIZE*0.5);	// upper-left of patch
		int c = round(keypts[k].pt.x) - floor(WIN_SIZE*0.5);
		for(int i=0;i<WIN_SIZE;i++)
		{
			for(int j=0;j<WIN_SIZE;j++)
			{					
				a = i*WIN_SIZE*3+j*3; 				
				desc.at<float>(k,a+0) = color.at<Vec3b>(r+i,c+j)(0)/255.f;
				desc.at<float>(k,a+1) = color.at<Vec3b>(r+i,c+j)(1)/255.f;
				desc.at<float>(k,a+2) = color.at<Vec3b>(r+i,c+j)(2)/255.f;
			}
		}
	}
	
	if(VERBOSE) {
		t = (getTickCount()-t)/getTickFrequency();
		if(space=="HSV")  cout << " copy HSV features:" << t << " secs." << endl;
		else if(space=="LAB")  cout << " copy LAB features:" << t << " secs." << endl;
	}
}

Mat perpix::getLabels(vector<KeyPoint> keypts, Mat im){
	
	//vector <float> v;
	Mat lab = Mat::zeros((int)keypts.size(),1,CV_32FC1);
	for(int i=0;i<(int)keypts.size();i++){
		Point p (keypts[i].pt.x,keypts[i].pt.y);
		if((int)im.at<uchar>(p.y,p.x)>0) lab.at<float>(i,0) = 1.0;
		//if((int)im.at<uchar>(p.y,p.x)>0) v.push_back(1.f);
		//else v.push_back(0);
	}
	//Mat lab(v);
	return lab;
}

Mat perpix::appendStackedResponse(Size sz, vector<KeyPoint> &keypts, Mat &desc, string method, int level){
	
	Mat f_x = Mat::zeros(desc.rows,1,CV_32FC1);
	
	if(level==0) return f_x;
	
	if(!desc.data){cout << "ERROR: no base features. abort." << endl;exit(1);}
	if((int)keypts.size()!=(int)desc.rows){cout << "ERROR: Number of kpts and data do not match." << endl;exit(1);}
	
	
	
	for(int i=1;i<=level;i++)
	{
		double t = getTickCount();
		
		Mat response = computeSingleLayerResponse(sz,keypts,desc,method,i-1,f_x);
		
		if(VISUALIZE){
			stringstream ss;
			ss << "perPix (skin) - " << (i-1) << endl;
			Mat dsp;
			colormap(response,dsp,0);
			imshow(ss.str(),dsp);
		}
		
		// remove zero patches
		//cout << "before:" << keypts.size() << endl;
		//removeZeroRegions(response,keypts,desc);
		//cout << "after:" << keypts.size() << endl;
		
		Mat avgRes = appendAverageResponse(response,sz,keypts,desc);
		
		if(0){
			stringstream ss;
			ss << "avgPix (skin) - " << (i-1) << endl;
			Mat dsp;
			colormap(avgRes,dsp,0);
			imshow(ss.str(),dsp);
		}
		
		t = (getTickCount()-t)/getTickFrequency();
		if(VERBOSE) cout << i << " compute response:" << t << " secs." << endl;
	}
	
	return f_x;
}

//Cheng Li
perpix::perpix(int argc, char * const argv[])
{
	string root = LC_ROOT_STRING;
	string train_basename = "GOPR2335"; //"GOPR2335"; //"GOPR0979";
	string test_basename = "GOPR2335"; //"GOPR2335"; //"GOPR0979";
	string ext = ".MP4";
	string msktyp = "msk_fg/";

	METHOD = "rdt";		// dt,gbt,ada,em, rdt

	int istrain = 1;

	int m_start_frame;
	int m_end_frame;
	int m_rate;

	int stat_mode = 0;

	addition_id = 0;
	unsupervise_kernel_number = 3;

	for(int ind = 1;ind < argc;ind++)
	{
		/*** Parse switches ***/
		if (argv[ind][0] == '-') {
			switch (argv[ind][1]) {  
		case 'f': root = string(argv[++ind]);
			break;
		case 's': m_start_frame = atoi(argv[++ind]);
			break;
		case 'e': m_end_frame = atoi(argv[++ind]);
			break;
		case 'r': m_rate = atoi(argv[++ind]);
			break;
		case 't': train_basename = string(argv[++ind]);			
			break;
		case 'p': test_basename = string(argv[++ind]);
			break;
		case 'm': METHOD = string(argv[++ind]);
			break;
		case 'y': ext = string(argv[++ind]);
			break;
		case 'a':
			istrain = 1;
			break;
		case 'b':
			istrain = 0;
			break;
		case 'z':
			addition_id = atoi(argv[++ind]);
			break;
		case 'k':
			unsupervise_kernel_number = atoi(argv[++ind]);
			break;
		case 'w':
			stat_mode = atoi(argv[++ind]);
			break;
		default : printf("Unknown switch '%c'\n", argv[ind][1]);
			break;
			}
		}
	}

	

	FEAT_RGB   = 0;
	FEAT_HSV   = 0;		
	FEAT_LAB   = 0;
	FEAT_BRIEF = 0;	
	FEAT_ORB   = 0;
	FEAT_SIFT  = 0;		// 1 sec
	FEAT_LOC   = 0;

	switch(stat_mode)
	{
		case 1:
			FEAT_LAB   = 1;
			break;
		case 2:
			FEAT_HSV   = 1;
			break;
		case 3:
			FEAT_LAB   = 1;
			FEAT_BRIEF = 1;
			break;
		case 4:
			FEAT_HSV = 1;
			FEAT_BRIEF = 1;	
			break;
		case 5:
			FEAT_LAB   = 1;
			FEAT_ORB   = 1;
			FEAT_BRIEF = 1;
			break;
		case 6:
			FEAT_HSV = 1;
			FEAT_ORB   = 1;
			FEAT_BRIEF = 1;
			break;
		default:
			FEAT_HSV = 1;
			FEAT_BRIEF = 1;
			break;
	}

	init(METHOD);

	set_train(root,msktyp,train_basename,ext,m_start_frame,m_end_frame,m_rate);
		
	set_test(root,msktyp,test_basename,ext,m_start_frame,m_end_frame,m_rate);

	f_start = m_start_frame;
	f_end = m_end_frame;
	f_rate = m_rate;

	

	if(istrain)
	{
		printf("run train\n");

	
		run_train();
	}
	else
	{
		run_test();
	}
}

Mat perpix::appendDiffStackedResponse(Size sz, vector<KeyPoint> &keypts, Mat &desc, string method, int level, Mat &f_x){
	
	//cout << "perpix::appendDiffStackedResponse" << endl;
	
	//Mat 
	f_x = Mat::zeros(desc.rows,1,CV_32FC1);
	
	if(level==0) return f_x;
	
	if(!desc.data){cout << "ERROR: no base features. abort." << endl;exit(1);}
	if((int)keypts.size()!=(int)desc.rows){cout << "ERROR: Number of kpts and data do not match." << endl;exit(1);}
	
	Mat sum_res = Mat::zeros(sz,CV_32FC1);
	
	for(int i=1;i<=level;i++)
	{
		double t = getTickCount();
		
		Mat h_x;
		Mat response = computeSingleLayerResponse(sz,keypts,desc,method,i-1, h_x);
		
		//cout << h_x << endl;

		f_x += h_x;				// vector format
		sum_res += response;	// image format
		
		Mat p_x;
		Mat exp_fx;
		exp(sigmoid_factor*sum_res,exp_fx);
		p_x = exp_fx / (1+exp_fx);
		
		if(VISUALIZE)
		{
			Mat dsp;
			stringstream ss;
			ss << "perPix (skin) - " << (i-1) << endl;
			colormap(p_x,dsp,0);
			imshow(ss.str(),dsp);
		}
		
		//waitKey(0);
		
		// remove zero patches
		//cout << "before:" << keypts.size() << endl;
		//removeZeroRegions(response,keypts,desc);
		//cout << "after:" << keypts.size() << endl;

		//cout << "average response" << endl;
		
		//Mat avgRes = appendAverageDiffResponse(response,sz,keypts,desc); // differential response
		Mat avgRes = appendAverageDiffResponse(sum_res,sz,keypts,desc);		// additive response * best
		//Mat avgRes = appendAverageDiffResponse(p_x,sz,keypts,desc);		// probabilisitc response
		//Mat avgRes = appendAverageResponse(p_x,sz,keypts,desc); // average prob response
		
		if(0){
			stringstream ss;
			ss << "avgPix (skin) - " << (i-1) << endl;
			Mat dsp;
			colormap(avgRes,dsp,0);
			imshow(ss.str(),dsp);
		}
		
		t = (getTickCount()-t)/getTickFrequency();
		if(VERBOSE) cout << " " << i << "] compute response:" << t << " secs." << endl;
	}
	
	return sum_res;
	
}

void perpix::removeZeroRegions(Mat response, vector<KeyPoint> &keypts, Mat &desc){
	
	Mat _dsc;
	vector<KeyPoint> _kpts;
	
	for(int k=0;k<(int)desc.rows;k++)
	{
		int _r = round(keypts[k].pt.y);			// center point
		int _c = round(keypts[k].pt.x);		
		int r = _r - floor(WIN_SIZE*0.5);		// upper-left of patch
		int c = _c - floor(WIN_SIZE*0.5);
		Rect roi(c,r,WIN_SIZE,WIN_SIZE);
		Mat sub = response(roi);					// response of previous layer
		int nonzero = countNonZero(sub);
		
		if(nonzero>0){
			_kpts.push_back(keypts[k]);				// save keypoint
			Mat row = desc.row(k);					// save descriptor
			if(!_dsc.data) row.copyTo(_dsc);
			else _dsc.push_back(row);
			
		}
	}
	desc = _dsc;
	keypts = _kpts;
	
}

Mat perpix::computeSingleLayerResponse(Size sz, vector<KeyPoint> keypts, Mat &desc, string method, int level){
	Mat hx;
	return computeSingleLayerResponse(sz, keypts,desc,method,level,hx);	
}


Mat perpix::computeSingleLayerResponse(Size sz, vector<KeyPoint> keypts, Mat &desc, string method, int level, Mat &h_x){
	
	//cout << "computeSingleLayerResponse" << endl;
	
	Mat res = Mat::zeros(sz,CV_32FC1);
	Mat hx = Mat::zeros((int)desc.rows,1,CV_32FC1);
	
	for(int k=0;k<(int)desc.rows;k++)
	{
		int _r = round(keypts[k].pt.y);			// center
		int _c = round(keypts[k].pt.x);					
		Mat row = desc.row(k);					// passing reference
		
		if(method=="ada")
		{
			float val = adaboost[level].predict(row);
			res.at<float>(_r,_c) = val;
			hx.at<float>(k,0) = val;
		}
		else if(method=="dt")
		{
			CvDTreeNode *node;
			node = dtree[level].predict(row,Mat(),false);
			res.at<float>(_r,_c) = node->value;
			hx.at<float>(k,0) = node->value;
		}
		else if(method=="em"){
			
			double log_p_pos = em[0].predict(row)(0) + log(p_pos);
			double log_p_neg = em[1].predict(row)(0) + log(p_neg);
			
			//log_p_pos /= (1.0e+13);
			//log_p_neg /= (1.0e+13);
			
			if( isinf(log_p_pos) && isinf(log_p_neg) ){
				log_p_pos = -INFINITY;
				log_p_neg = 0;
			}
			else if(isinf(log_p_neg)){
				log_p_neg = 0;
			}
			else if(isinf(log_p_pos)){
				log_p_pos = -INFINITY;		// not skin
			}
			
			double diff = log_p_pos - log_p_neg;
			
			
			//cout << log_p_pos << " " << log_p_neg << " " << diff << " p:" << exp(diff) << endl;
			
			
			if(diff>0) diff = 0; // acts as ceiling function skin with p=1
			
			res.at<float>(_r,_c) = (float)exp(diff);
			
			//if( res.at<float>(_r,_c) > 0 && res.at<float>(_r,_c) <1 ) cout << diff << "," << res.at<float>(_r,_c) << endl;
			
			//cout << em[1].predict(row)(0) << endl;
			hx.at<float>(k,0) = res.at<float>(_r,_c);
			//if(hx.at<float>(k,0)>1.0) cout << hx.at<float>(k,0) << endl;
		}
		else exit(1);
		
	}
	h_x = hx;
	//cout << "done" << endl;
	return res;
}


void perpix::subsampleDescriptors(vector<KeyPoint> &keypts, Mat &desc,int step, int offset){
	Mat f_x = Mat();
	Mat lb = Mat();
	subsampleDescriptors(keypts,desc,step,1,offset,f_x,lb);
}

void perpix::subsampleDescriptors(vector<KeyPoint> &keypts, Mat &desc,int sample_step, int neg_step, int offset, Mat &f_x, Mat &lb)
{	
	
	cout << "[BEFORE] Number of keypoints: " << (int)keypts.size() << endl;
	cout << "[BEFORE] Number of desc: " << desc.rows << endl;
	cout << "[BEFORE] Number of labels: " << lb.rows << endl;
	cout << "[BEFORE] Number of f_x: " << f_x.rows << endl;
	
	Mat _dsc;
	Mat _lb;
	vector<KeyPoint> _kpts;
	Mat _fx;
	int j = offset;		// initial offset
	Mat row;
	
	int neg_i=0;
	int isneg = 0;
	int l=0;
	
	for(int k=0;k<(int)keypts.size();k++,j++)
	{
		if((j%sample_step)==0)
		{
			
			if(lb.data && lb.at<float>(k,0)==0) isneg = 1;
			else isneg = 0;
				
			if(isneg) neg_i++;	// negative feature counter			
			
			if(isneg && (neg_i%neg_step)!=0 ) continue;
			
			lb.at<float>(l++,0) = lb.at<float>(k,0);
			
			//cout << "lab:" << lb.at<float>(k,0) << endl;
			
			_kpts.push_back(keypts[k]);
			
			row = desc.row(k);					// add descriptor
			if(!_dsc.data) row.copyTo(_dsc);
			else _dsc.push_back(row);
			
			if(f_x.data){
				row = f_x.row(k);					// add response
				if(!_fx.data) row.copyTo(_fx);
				else _fx.push_back(row);
			}
		}
	}
	
	desc = _dsc;
	keypts = _kpts;
	f_x = _fx;
	lb = lb.rowRange(0,l)*1.0;
	
	cout << "[AFTER] Number of keypoints: " << (int)keypts.size() << endl;
	cout << "[AFTER] Number of desc: " << desc.rows << endl;
	cout << "[AFTER] Number of labels: " <<lb.rows << endl;
	cout << "[AFTER] Number of f_x: " << f_x.rows << endl;
	
	
}


void perpix::ConvexityDefects( const Mat& points, const vector<int>& hull, vector<CvConvexityDefect> &defects )
{
	int nelems = points.checkVector(2, CV_32S);
	int nhelems = (int)hull.size();
    if (nelems < nhelems)
	{
		defects.clear();
		return;
	}
	
	// convert to CvMat (C API)
	CvMat _points = Mat(points), _hull=Mat(hull);
	// allocate storage for the defects
	MemStorage storage(cvCreateMemStorage());
    Seq<CvConvexityDefect> defectSeq( cvConvexityDefects(&_points, &_hull, storage) );
	defectSeq.copyTo(defects);
}


void perpix::trainModel(Mat &data, Mat &lab, string model_filename, string method){
	
	// PCA
	
	
	double t;
	// === train 2nd models === //
	if(method=="svm")
	{
		CvSVMParams sv_params;
		sv_params.term_crit=cvTermCriteria(CV_TERMCRIT_ITER,100,0.000001);
		sv_params.kernel_type=CvSVM::LINEAR;
		sv_params.svm_type=CvSVM::EPS_SVR;
		sv_params.p = 1;
		
		if(VERBOSE) cout << "Train SVM ...";
		t = getTickCount();
		svm.train_auto(data,lab,Mat(),Mat(),sv_params);
		t = (getTickCount()-t)/getTickFrequency();
		if(VERBOSE) cout << " time:" << t << " secs." << endl;
		
	}	
	else if(method=="em")
	{
		
		Mat pos;
		Mat neg;
		
		if(VERBOSE) cout << "Spliting data ..." << endl;
		for(int i=0;i<data.rows;i++){
			if(lab.at<float>(i,0)>0){
				if(!pos.data) data.row(i).copyTo(pos);
				else pos.push_back(data.row(i));
			}
			else {
				//if(i%10!=0) continue;
				if(!neg.data) data.row(i).copyTo(neg);
				else neg.push_back(data.row(i));

			}
		}
		
		// preprocessing with PCA should help (since assuming diagonal covariance)?
		int nclusters[2];
		nclusters[0] = 16;
		nclusters[1] = 16;
		//CvTermCriteria term_crit=cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, FLT_EPSILON);
		//CvEMParams param[2];
		//param[0] = CvEMParams(nclusters[0]);
		//param[1] = CvEMParams(nclusters[1]);
		
		
		FileStorage fs;
		
		if(VERBOSE) cout << "Training with "<< pos.rows <<" positive EM model...";
		t = getTickCount();
		em[0] = EM::EM(nclusters[0]);
		em[0].train(pos);
		//em[0].train(pos, Mat(),param[0]);
		t = (getTickCount()-t)/getTickFrequency();
		cout << " time:" << t << " secs." << endl;
		
		string posmodel = model_filename + "_pos.yml";
		fs = FileStorage(posmodel.c_str(), FileStorage::WRITE);
		em[0].write(fs);
        //em[0].write(fs.fs, "EM");
		
		
		if(VERBOSE) cout << "Training  with " << neg.rows << " negative EM model...";
		t = getTickCount();
		em[1] = EM::EM(nclusters[1]);
		em[1].train(neg);
		//em[1].train(neg, Mat(),param[1]);
		t = (getTickCount()-t)/getTickFrequency();
		cout << " time:" << t << " secs." << endl;
		
		string negmodel = model_filename + "_neg.yml";
		fs = FileStorage(negmodel.c_str(), FileStorage::WRITE);
        //em[1].write(fs.fs, "EM");
		em[1].write(fs);
		
	}
	else if(method=="dt")
	{
		CvDTree dtr;
		CvDTreeParams dt_params = CvDTreeParams();
		//		dt_params = CvDTreeParams(12, // 9, max depth
		//								  10, // min sample count
		//								  0.0, // regression accuracy (non-zero value makes it regression?)
		//								  true, // compute surrogate split, as we have missing data
		//								  12, //9, (not used for regression) max number of categories (use sub-optimal algorithm for larger numbers)
		//								  4, // the number of cross-validation folds
		//								  true, // true: harsh pruning
		//								  true, // true: truncate tree
		//								  0 // the array of priors, the bigger p_weight, the more attention
		//								  );
		
		dt_params = CvDTreeParams(TREE_DEPTH,10,0.0,true,TREE_DEPTH,4,true,true,0);
		//dt_params = CvDTreeParams(12,10,0.0, true,12,4,true,true,0);
		//dt_params = CvDTreeParams(9,10,0.0, true,9,4,true,true,0);
		//dt_params = CvDTreeParams(6,10,0.0, true,9,4,true,true,0);
		//dt_params = CvDTreeParams(4,10,0.0, true,10,4,true,true,0);
		
		if(VERBOSE) cout << "num data:" << data.rows << endl;
		
		Mat varType = Mat::ones(data.cols+1,1,CV_8UC1) * CV_VAR_NUMERICAL; // all floats
		
		double t = getTickCount();
		if(VERBOSE) cout << "Train decision tree model ...";
		dtr.train(data,CV_ROW_SAMPLE,lab,Mat(),Mat(),varType,Mat(),dt_params);
		t = (getTickCount()-t)/getTickFrequency();
		if(VERBOSE) cout << " time:" << t << " secs." << endl;
		
		if(VERBOSE) cout << "Saving model to: " << model_filename << endl;
		string dtmodel = model_filename + ".yml";
		dtr.save(dtmodel.c_str());
		
	}
	else if(method=="ada")
	{
		CvBoost atr;
		CvBoostParams params;
		int boost_type = CvBoost::GENTLE; //CvBoost::REAL; //CvBoost::GENTLE;
		int weak_count = 100;
		double weight_trim_rate = 0.95;
		int max_depth = 1;
		bool use_surrogates = false;
		const float* priors = NULL;
		params = CvBoostParams(boost_type, weak_count,weight_trim_rate,max_depth,use_surrogates,priors);
		
		Mat varType = Mat::ones(data.cols+1,1,CV_8UC1) * CV_VAR_NUMERICAL; // all floats
		varType.at<uchar>(data.cols,0) = CV_VAR_CATEGORICAL;
		
		//lab = lab*2-1;
		//cout << lab << endl;
		//lab.convertTo(lab,CV_8UC1);
		
		double t = getTickCount();
		if(VERBOSE) cout << "Train (Gentle) AdaBoost model ...";
		atr.train(data,CV_ROW_SAMPLE,lab,Mat(),Mat(),varType,Mat(),params,false);
		t = (getTickCount()-t)/getTickFrequency();
		if(VERBOSE) cout << " time:" << t << " secs." << endl;
		
		if(VERBOSE) cout << "Saving model to: " << model_filename << endl;
		string model = model_filename + ".yml";
		atr.save(model.c_str());
		cout << "Done saving model." << endl;
		
	}
	
}

string perpix::getModelString(){
	
	string model_code;
	stringstream s;
	if(FEAT_RGB)   model_code += "_RGB";
	if(FEAT_HSV)   model_code += "_HSV";
	if(FEAT_LAB)   model_code += "_LAB";
	if(FEAT_BRIEF) model_code += "_BRIEF";
	if(FEAT_ORB)   model_code += "_ORB";
	if(FEAT_SIFT)   model_code += "_SIFT";
	
	//s.str(""); s << WIN_SIZE;
	//Cheng Li
	if(addition_id==0){ s.str(""); s << WIN_SIZE; }
	else{ s.str(""); s << WIN_SIZE << "_id" << addition_id ; }	
	
	
	model_code += "_W"+s.str();
	model_code += "_"+METHOD;
	if(ADDITIVE) model_code += "_ADD";
	if(LIGHT_COMP) model_code += "_LC";
	if(FEAT_LOC) model_code += "_LOC";		
	
	return model_code;
}

Mat perpix::appendAverageResponse(Mat response, Size sz, vector<KeyPoint> keypts, Mat &desc){
	
	Mat avgRes = Mat::zeros(sz,CV_32FC1);
	
	Mat avg_feat;		
	avg_feat = Mat::zeros(1,desc.rows,desc.type());
	for(int k=0;k<(int)desc.rows;k++)
	{
		int _r = round(keypts[k].pt.y);			// center point
		int _c = round(keypts[k].pt.x);		
		int r = _r - floor(WIN_SIZE*0.5);		// upper-left of patch
		int c = _c - floor(WIN_SIZE*0.5);
		Rect roi(c,r,WIN_SIZE,WIN_SIZE);
		Mat sub = response(roi);					// response of previous layer
		//sub *= 0;
		Scalar avg = mean(sub);
		
		//cout << avg(0) << endl;
		
		//if(avg(0)!=INFINITY){
			avg_feat.at<float>(0,k) = avg(0);		// store local average response
			avgRes.at<float>(_r,_c) = avg(0);
		//}
		//else {
			avg_feat.at<float>(0,k) = 0;		// store local average response
			avgRes.at<float>(_r,_c) = 0;
		//}

	}
	
	desc = desc.t();
	desc.push_back(avg_feat);
	desc = desc.t();
	
	return avgRes;
}

Mat perpix::appendAverageDiffResponse(Mat response, Size sz, vector<KeyPoint> keypts, Mat &desc){
	
	Mat avgRes = Mat::zeros(sz,CV_32FC1);
	
	Mat avg_feat;		
	avg_feat = Mat::zeros(1,desc.rows,desc.type());
	for(int k=0;k<(int)desc.rows;k++)
	{
		int _r = round(keypts[k].pt.y);			// center point
		int _c = round(keypts[k].pt.x);		
		int r = _r - floor(WIN_SIZE*0.5);		// upper-left of patch
		int c = _c - floor(WIN_SIZE*0.5);
		Rect roi(c,r,WIN_SIZE,WIN_SIZE);
		Mat sub = response(roi);					// response of previous layer
		Scalar avg = mean(sub);
		float val = response.at<float>(_r,_c);
		avg_feat.at<float>(0,k) = val - avg(0);		// store local average response
		avgRes.at<float>(_r,_c) = val - avg(0);
	}
	
	desc = desc.t();
	desc.push_back(avg_feat);
	desc = desc.t();
	
	return avgRes;
}


Mat perpix::loadGT(int f,string basename)
{
	stringstream ss;
	ss << root + msktyp + basename + "/" + basename + "_maskgt_" <<f<<".jpg";
	//ss << root + msktyp + basename + "/" + basename + "_contgt_" <<f<<".jpg";
	Mat im = imread(ss.str(),0);
	if(!im.data){ 
		cout << "ERROR: missing ground truth for frame: " << f << endl;
		cout << ss.str() << endl;
		exit(1);
	}
	threshold(im,im,253,255,THRESH_BINARY);		// remove jpeg artifacts
	medianBlur(im,im,3);						// remove holes
	return im;
}

void perpix::postprocessing(Mat src, Mat response){
	
	if(0){
		string filename;
		FileStorage fs;
		
		cout << "Load hand_db" << endl;
		filename = "/video/FirstPersonVideos/img/hand_db/hand_features.yml";
		fs.open(filename, FileStorage::READ);
		
		fs["feat"] >> feat;		
		feat.convertTo(feat,CV_32FC1,1.0,-0.5);
		
		filename = "/video/FirstPersonVideos/img/hand_db/wrist_xy.yml";
		fs.open(filename, FileStorage::READ);
		
		fs["wristxy"] >> wristxy;
		
		wristxy.convertTo(wristxy,CV_32FC1);
		
		// ===== Train ANN model ===== //
		cout << "Train hand_db" << endl;
		
		//flann::KDTreeIndexParams kdIdxParam(16); cout << "Train kdTree.\n";
		
		flann::LinearIndexParams IdxParam;
		cout << "Train Linear (brute force).\n";
		//cv::flann::Index flannIndex(feat,IdxParam);
		flannIndex = cv::flann::Index(feat,IdxParam);
		
		cout << "Save hand_db" << endl;
		filename = "/video/FirstPersonVideos/img/hand_db/hand_idx.dat";
		flannIndex.save(filename.c_str());
	}
	// =========================== //
	//Point bound_ul(src.cols,src.rows);
	//Point bound_br(0,0);
	
	// ==== POST PROCESSING ==== //
	
	if(0){
		
		int win = 11;
		
		//Mat bin;
		//threshold(prob[0],bin,0.90,255,THRESH_BINARY);
		//bin.convertTo(bin,CV_8UC1);
		//medianBlur(bin,bin,win);
		//imshow("bin",bin);
		
		Mat bin90;
		threshold(response,bin90,0.90,255,THRESH_BINARY);
		bin90.convertTo(bin90,CV_8UC1);
		medianBlur(bin90,bin90,win);
		//imshow("bin90",bin90);
		
		Mat bin80;
		threshold(response,bin80,0.80,255,THRESH_BINARY);
		bin80.convertTo(bin80,CV_8UC1);
		medianBlur(bin80,bin80,win);
		//imshow("bin80",bin80);
		
		Mat mask = Mat::ones(response.size(),CV_8UC1)*GC_PR_BGD;		// probably background
		
		Mat prFGD = Mat::ones(response.size(),CV_8UC1)*GC_PR_FGD;
		prFGD.copyTo(mask,bin80);
		
		Mat FGD = Mat::ones(response.size(),CV_8UC1)*GC_FGD;
		FGD.copyTo(mask,bin90);
		
		Mat BGD = Mat::ones(response.size(),CV_8UC1)*GC_BGD;
		Mat border = Mat::zeros(response.size(), CV_8UC1);
		rectangle(border,Point(0,0),Point(border.cols-1,border.rows+64),Scalar::all(255),100);
		BGD.copyTo(mask,border);
		
		Mat bgd,fgd;
		
		if(countNonZero(bin90)>0) grabCut(src,mask, Rect(), bgd, fgd, 1, cv::GC_INIT_WITH_MASK);
		
		Mat binmask = getBinMask(mask) * 255;
		
		stringstream ss;
		//ss.str("");
		//ss << root<<"img/"<<test_basename<<"_skinout/"<<test_basename<<model_code<<"_maskout"<<f<<".jpg";
		
		vector<int> q;
		q.push_back(CV_IMWRITE_JPEG_QUALITY);
		q.push_back(100);
		imwrite(ss.str(),binmask,q);
		
		Mat dst;
		if(VISUALIZE){
			Mat skin;
			cvtColor(binmask,skin,CV_GRAY2BGR);
			addWeighted(src,0.5,skin,0.5,0,dst);
			imshow("gcskin",dst);
		}
		
		// ==== Connect components ==== // get bounding rectangle
		
		if(1){
			
			vector<vector<Point> > co;
			vector<Vec4i> hi;
			Mat contmap;
			binmask.copyTo(contmap);
			findContours(contmap,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
			
			Mat dsp;
			if(VISUALIZE) dst.copyTo(dsp);
			
			//vector < vector<int> > hull;
			//vector<Mat> cont;
			
			int offset = 0;
			Mat arm = Mat::zeros(400,400,CV_8UC3);
			
			for(int i=0;i<co.size();i++)
			{
				
				Rect b = boundingRect(Mat(co[i]));
				
				if(b.x<100) continue;
				if(contourArea(Mat(co[i])) < 1000) continue;
				cout << i << "] " << contourArea(Mat(co[i])) << endl; 
				
				
				// === Get bounding min rectangle === //
				RotatedRect minRect(minAreaRect(Mat(co[i])));
				Point2f rect_points[4];
				minRect.points( rect_points );
				if(VISUALIZE){
					for( int j = 0; j < 4; j++ ){
						//if(j==2) circle(dsp,rect_points[j],4,CV_RGB(255,0,0),-1);
						//if(j==3) circle(dsp,rect_points[j],4,CV_RGB(255,255,255),-1);
						line( dsp, rect_points[j], rect_points[(j+1)%4], CV_RGB(0,255,0), 1,CV_AA );
					}
				}
				
				
				// ===== FIT LINE ==== //
				Point2f spts[4];
				float max_dist = (float)src.cols;
				int idx;
				for( int j = 0; j < 4; j++ ){
					Point2f p = rect_points[j];
					Point2f q = rect_points[(j+1)%4];
					float dist = hypot(p.x-q.x, p.y-q.y);
					if(dist<max_dist){
						idx=j;
						max_dist = dist;
					}
				}
				
				spts[0] = rect_points[idx];
				spts[1] = rect_points[(idx+1)%4];
				spts[2] = rect_points[(idx+2)%4];
				spts[3] = rect_points[(idx+3)%4];
				
				line( dsp,spts[0],spts[1], CV_RGB(255,255,0), 2,CV_AA );
				line( dsp,spts[2],spts[3], CV_RGB(255,255,0), 2,CV_AA );
				
				Vec4f mline;
				fitLine(Mat(co[i]),mline,CV_DIST_L2,0, 0.01,0.01);
				
				Point p[2];
				for(int j=0;j<2;j++)
				{
					Point2f pt[4];
					
					if(j==0){
						pt[0] = spts[0];
						pt[1] = spts[1];
					}
					else{
						pt[0] = spts[2];
						pt[1] = spts[3];
					}
					
					pt[2] = Point2f(mline(2),mline(3)); 
					pt[3] = Point2f(mline(2)+mline(0),mline(3)+mline(1));
					
					p[j].x = (
							  (pt[0].x*pt[1].y - pt[0].y*pt[1].x)*(pt[2].x-pt[3].x) 
							  - 
							  (pt[0].x-pt[1].x)*(pt[2].x*pt[3].y - pt[2].y*pt[3].x) 
							  ) 
					/ 
					(
					 (pt[0].x-pt[1].x)*(pt[2].y-pt[3].y) 
					 - 
					 (pt[0].y-pt[1].y)*(pt[2].x-pt[3].x)
					 );
					p[j].y = (
							  (pt[0].x*pt[1].y - pt[0].y*pt[1].x)*(pt[2].y-pt[3].y) 
							  - 
							  (pt[0].y-pt[1].y)*(pt[2].x*pt[3].y - pt[2].y*pt[3].x) 
							  ) 
					/ 
					(
					 (pt[0].x-pt[1].x)*(pt[2].y-pt[3].y) 
					 - 
					 (pt[0].y-pt[1].y)*(pt[2].x-pt[3].x)
					 );					
					
					//circle(dsp, p[j],3,CV_RGB(0,0,255),2);
				}
				
				line(dsp,p[0],p[1],CV_RGB(0,0,255),2,CV_AA);
				
				
				
				// ===== Get orientation ===== //
				Point c = minRect.center;
				float _deg = 180;
				//int idx;
				
				for( int j = 0; j < 4; j++ ){
					float deg = fastAtan2(rect_points[j].y-c.y, rect_points[j].x-c.x);
					if(deg>_deg){	// note: coordinates are upside down in images
						idx=j;
						break;
					}
				}
				
				Point2f srcpt[4];
				Point2f dstpt[4];
				for( int j = 0; j < 4; j++ ) srcpt[j] = rect_points[(idx+j)%4];
				
				int w = minRect.size.height;
				int h = minRect.size.width;
				
				
				
				//int w = 50;
				//float a = 50.f / minRect.size.height;
				//int h = round( minRect.size.width * a);
				
				
				
				if((idx%2)){
					
					w = minRect.size.width;
					h = minRect.size.height;
					//h = 50;
					//a = 50.f / minRect.size.width;
					//w = round( minRect.size.height * a);
					
				}
				
				//=== normalize image === //
				float a = 100.f / w;
				w = 100;
				h = round(a*h);
				
				dstpt[0]=Point(offset+0,0);
				dstpt[1]=Point(offset+w,0);
				dstpt[2]=Point(offset+w,h);
				dstpt[3]=Point(offset+0,h);
				
				offset+=w;
				
				Mat A = getAffineTransform(srcpt,dstpt);
				
				Mat tmp;
				Mat bin;
				warpAffine(src,tmp,A,arm.size(),INTER_LINEAR);
				warpAffine(binmask,bin,A,arm.size(),INTER_LINEAR);
				
				h=300;
				Rect sub(dstpt[0].x,dstpt[0].y,w,h);
				
				//cout << "sub:" << sub.tl() << " " << sub.br() << endl;
				
				Mat tmpsub = tmp(sub);			// warped image
				Mat armsub = arm(sub);			// display image
				Mat binsub = bin(sub);		// display image
				tmpsub.copyTo(armsub,binsub);
				
				
				if(SAVE_HAND_DB){
					vector<int> q;
					q.push_back(CV_IMWRITE_JPEG_QUALITY);
					q.push_back(100);
					
					ss.str("");
					ss << root << "img/hand_db/" << db++ << ".jpg";
					imwrite(ss.str(),armsub,q);
				}
				else {
					
					Mat tmp;
					cvtColor(armsub,tmpsub,CV_BGR2GRAY);
					threshold(tmpsub,tmpsub,10,1,THRESH_BINARY);
					medianBlur(tmpsub,tmpsub,31);	// 3
					tmpsub.convertTo(tmpsub,CV_32FC1,1.0, -0.5);
					//cout << tmpsub << endl;
					
					imshow("query",tmpsub);
					
					Mat query = tmpsub.reshape(1,1);
					Mat ind;
					Mat dis;
					int knn = 1;
					flannIndex.knnSearch(query,ind, dis,knn, flann::SearchParams(64));
					
					//cout << "ind:" << ind << endl;
					//cout << "dis:" << dis << endl;
					
					int index = (int)ind.at<uchar>(0,0);
					
					
					Mat db;// = feat.row(index);
					feat.row(index).copyTo(db);
					db = db.reshape(1, 300);
					//db = (db + 0.5)*255;
					db *= 255;
					db.convertTo(db,CV_8UC1);
					imshow("db_hit",db);
					waitKey(1);
					
					// draw wrist position //
					armsub = arm(sub);
					//cout << "xy:" << wristxy.row(index) << endl;
					int x = (int)wristxy.at<float> ( index, 0 )*2;
					int y = (int)wristxy.at<float> ( index, 1 );
					//cout <<Point(x,y) << endl;
					circle(armsub,Point(x,y),3,CV_RGB(255,0,0),-1);
					
				}
				
				
				// ==== Find local min point ==== //
				
				//					warpAffine(binmask,tmp,A,arm.size(),INTER_LINEAR);
				//					tmpsub = tmp(sub);
				//					vector<float> v;
				//					for(int j=0;j<tmpsub.rows;j++){
				//						Mat row = tmpsub.row(j);
				//						Scalar s = sum(row);
				//						v.push_back(s(0));
				//					}
				//					Mat profile = Mat(v);
				//					//cout << profile << endl;
				//					
				//					GaussianBlur(profile,profile,Size(1,5),0,0,BORDER_CONSTANT);
				
				//medianBlur(profile,profile,11);
				//cout << profile << endl;
				
				//	float minmax_val=0;
				//					for(int j=profile.rows-20;j>0;j--){
				//						if(profile.at<float>(j,0)<minmax_val) minmax_val = profile.at<float>(j,0);
				//						else {
				//							idx = j;		// found first dip/jump
				//							break;
				//						}
				//					}
				
				
				// TODO: wrist detection!!!!
				//Point2f wrist;
				//wrist.x = dstpt[0].x + w*0.5;
				//wrist.y = idx;
				
				
				
				
				//cout << "A:" << A << endl;
				
				//Mat R = A(Rect(0,0,2,2)) * 1.0;
				//R = R.inv();
				
				//cout << "R:" << R << endl;
				
				//wrist.x = R.at<float>(0,0)*wrist.x + R.at<float>(0,1)*wrist.y + dstpt[0].x + A.at<float>(0,2);  
				//wrist.y = R.at<float>(1,0)*wrist.x + R.at<float>(1,1)*wrist.y + dstpt[0].y + A.at<float>(1,2);  
				
				//circle(arm, wrist,5,CV_RGB(255,255,255),1,CV_AA);
				
				if(VISUALIZE)
					imshow("arm",arm);
				
				
				if(0)
				{
					
					vector<int> hu;
					convexHull(Mat(co[i]),hu);
					vector<CvConvexityDefect> defects;
					ConvexityDefects(Mat(co[i]),hu, defects);
					
					for(int j=0;j<(int)defects.size();j++){
						Point p = *(defects[j].depth_point);
						Point s = *(defects[j].start);
						Point e = *(defects[j].end);
						float d = defects[j].depth;
						
						if(d<6) continue;
						
						if(hypot(s.x-p.x, s.y-p.y) > 50) continue;
						if(hypot(e.x-p.x, e.y-p.y) > 50) continue;
						
						line(dsp,p,s,CV_RGB(0,255,0),3);
						line(dsp,p,e,CV_RGB(0,255,0),3);
						circle(dsp,p,3,CV_RGB(0,255,255),-1,CV_AA);
						circle(dsp,s,3,CV_RGB(255,0,255),-1,CV_AA);
						circle(dsp,e,3,CV_RGB(255,255,0),-1,CV_AA);
					}
				} // convex hull processing
				
			}
			imshow("dsp",dsp);
		}// if connect components
		
		//if(!avi.isOpened()){
		//	string vidout = root+"vis/"+test_basename+model_code+".avi";
		//	avi.open(vidout,CV_FOURCC('F','L','V','1'),30,dst.size(), true);
		//}
		//else avi << dst;
		
	}// if grabcut on/off
	
	// ==== Compute LOSS ==== //
	
	//			int islabeled = 1;
	//			ss.str("");
	//			ss <<"/video/FirstPersonVideos/img/"<<test_basename<<"_skin/"<<test_basename<<"_skin_"<<f<<".jpg";
	//			Mat im = imread(ss.str(),0);
	//			if(!im.data) islabeled = 0;
	//			
	//			if(islabeled)
	//			{
	//				Rect roi(bound_ul.x,bound_ul.y,bound_br.x-bound_ul.x,bound_br.y-bound_ul.y);
	//				
	//				threshold(im,im,250,255,THRESH_BINARY);		// remove jpeg artifacts
	//				medianBlur(im,im,3);						// remove holes
	//				
	//				Mat gt;
	//				im(roi).convertTo(gt,CV_8UC1);
	//				
	//				Mat bi = binmask(roi);
	//				Mat loss;
	//				bitwise_xor(bi,gt,loss);
	//				int zero_one_loss = countNonZero(loss);		
	//				
	//				Mat pos;
	//				bitwise_and(bi,gt,pos);
	//				float precision = countNonZero(pos)/(float)countNonZero(bi);
	//				float recall = countNonZero(pos)/(float)countNonZero(im);
	//				float fscore = 2.0*precision*recall / (float)(precision+recall);
	//			
	//				cout << f << "] ";
	//				cout << "Precision / Recall / F-score / Loss: ";
	//				cout << precision << " ";
	//				cout << recall << " ";
	//				cout << fscore << " ";
	//				cout << zero_one_loss << " ";
	//				cout << endl;
	//				
	//				fout << f << "\t";
	//				fout << precision << "\t";
	//				fout << recall << "\t";
	//				fout << fscore << "\t";
	//				fout << zero_one_loss << "\t";
	//				fout << endl;
	//			}
	
	//imshow("src",src);
	
	//colormap(prob[0],dst);
	//imshow("skin",dst);
	
	//if(method=="em"){
	//	colormap(prob[1],dst);
	//	imshow("nonskin",dst);
	//}

//	void perpix::run_train_aggregate()
//	{	
//		
//		string filename = root+"out/"+train_basename+model_code+"_train_loss.txt";
//		ofstream fileout(filename.c_str());
//		fileout << "level\tloss\tprecision\trecall\tfmeasure\n";
//		
//		Mat frm, src;
//		VideoCapture cap;
//		
//		if(!FEAT_ORB && !FEAT_BRIEF) initImgBound = WIN_SIZE;
//		
//		DenseFeatureDetector dfd;
//		dfd = DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul,train_initXyStep,initImgBound,true,false);
//		
//		for(int k=train_start;k<NUM_STACK;k++)
//		{
//			cout << "k=" << k << endl;
//			
//			loadModels(k);
//			
//			Mat dats;
//			Mat labs;
//			Mat dsp;
//			Mat f_x;
//			
//			cap.open(root+"vid/"+train_basename+ext);
//			
//			int f = -1;
//			
//			while(waitKey(1)!=27)
//			{
//				cap >> frm;
//				if(!frm.data) break;
//				f++;
//				
//				if( ((f%f_rate)!=0) || (f<f_start) ) continue;
//				if(f>f_end) break;
//				
//				resize(frm,src,Size(),scale,scale);
//				imshow("src",src);
//				
//				
//				Mat im = loadGT(f,train_basename);
//				if(train_basename.find("GOPR")==string::npos) resize(im,im,Size(),scale,scale, INTER_NEAREST);
//				
//				imshow("im",im);
//				
//				if(VISUALIZE){
//					src.copyTo(dsp,im);
//					imshow("ground truth",dsp);
//				}
//				
//				vector<KeyPoint> keypts;
//				Mat desc;
//				
//				dfd.detect(src,keypts);
//				
//				appendFeatures(src,keypts,desc);
//				Mat fx = appendStackedResponse(src.size(),keypts,desc,METHOD,k);
//				
//				subsampleDescriptors(keypts,desc,NUM_STACK*SAMPLE_STEP,k,fx);						// every layer trained on different pixels			
//				
//				Mat lb = getLabels(keypts,im);
//				
//				cout << "store" << endl;
//				//store(desc,lb);
//				if(!dats.data) desc.copyTo(dats);
//				else dats.push_back(desc);
//				if(!labs.data) lb.copyTo(labs);
//				else labs.push_back(lb);
//				if(!f_x.data) fx.copyTo(f_x);
//				else f_x.push_back(fx);
//				
//				cout << f << "] data:" << dats.rows << " skin:" << countNonZero(labs) << " dims:" << dats.cols << endl;
//				
//				
//				
//			} // while loop (video)
//			
//			cout << "Data dims: " << dats.cols << endl;
//			if(VISUALIZE) destroyAllWindows();		
//			
//			stringstream modelfile;
//			modelfile << root << "models/" << train_basename << model_code << "_" << k;
//			trainModel(dats,labs,modelfile.str(),METHOD);
//			
//			if(k==0) f_x += 0.5;
//			
//			//========================================================//
//			
//			cout << "==========================" << endl;
//			
//			Mat bin;
//			threshold(f_x,bin,0.49,1.0,THRESH_BINARY);
//			Mat zeroOneLoss = labs - bin;
//			
//			Mat pos;
//			bin.convertTo(pos,CV_8UC1);			// all positive responses
//			Mat tp;
//			labs.copyTo(tp,pos);				// all true positives in positive responses
//			
//			cout << "detected positives:" <<  sum(pos)(0) << endl;
//			cout << "detected true positives:" <<  sum(tp)(0) << endl;
//			cout << "all true positives:" <<  sum(labs)(0) << endl;
//			
//			float precision = sum(tp)(0) / sum(pos)(0);
//			float recall = sum(tp)(0) / sum(labs)(0);
//			float fmeasure = 2.0*precision*recall / (precision+recall);
//			
//			cout << "Zero One Loss: " << sum(abs(zeroOneLoss))(0) << endl;
//			cout << "Precision: " << precision << endl;
//			cout << "Recall: " << recall << endl;
//			cout << "Fmeasure: " << fmeasure << endl;
//			
//			fileout << k;
//			fileout << "\t" << sum(abs(zeroOneLoss))(0);
//			fileout << "\t" << precision;
//			fileout << "\t" << recall;
//			fileout << "\t" << fmeasure;
//			fileout << endl;
//			
//			if(METHOD=="em"){
//				p_pos = sum(labs)(0) / labs.rows;
//				p_neg = sum(1-labs)(0) / labs.rows;
//				cout << "p_pos: " << p_pos << endl;
//				cout << "p_neg: " << p_neg << endl;
//				string filename = root+"models/"+train_basename+model_code+"_prior.xml";
//				ofstream fs(filename.c_str());
//				fs << p_pos << endl;
//				fs << p_neg << endl;
//			}
//		}
//	}
//	
//	void perpix::run_train_additive()
//	{	
//		
//		if(!TRAIN_ON) return;
//		
//		cout << "Training additive model" << endl;
//		
//		string filename = root+"out/"+train_basename+model_code+"_train_loss.txt";
//		ofstream fileout(filename.c_str());
//		
//		Mat frm, src;
//		VideoCapture cap;
//		
//		if(!FEAT_ORB && !FEAT_BRIEF) initImgBound = WIN_SIZE;
//		
//		DenseFeatureDetector dfd;
//		dfd = DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul,train_initXyStep,initImgBound,true,false);
//		
//		for(int k=train_start;k<NUM_STACK;k++)
//		{
//			cout << "k=" << k << endl;
//			loadModels(k);
//			
//			Mat dats;
//			Mat labs;
//			Mat f_x;				// gradient
//			
//			Mat gif;		// global illumination feature (GIF)
//			// top LUMA pixels average R.G.B. (make each average to be 255)
//			
//			cap.open(root+"vid/"+train_basename+ext);
//			int f = -1;
//			
//			while(waitKey(1)!=27)
//			{
//				
//				cap >> frm;
//				if(!frm.data) break;
//				f++;
//				
//				if( ((f%f_rate)!=0) || (f<f_start) ) continue;
//				if(f>f_end) break;
//				
//				resize(frm,src,Size(),scale,scale);
//				imshow("src",src);
//				
//				// ==== illumination feature ==== //
//				if(LIGHT_COMP){
//					Mat gry;
//					cvtColor(src,gry,CV_BGR2GRAY);
//					double maxVal = 255.*0.95;
//					threshold(gry,gry,maxVal,255,THRESH_BINARY);
//					Mat top5;
//					src.copyTo(top5,gry);
//					imshow("top5",top5);
//					Scalar s = mean(top5,gry);
//					
//					if(countNonZero(gry)>100)
//					{
//						vector<Mat> mv;
//						split(src,mv);
//						mv[0] = mv[0] / s(0) * 255.;
//						mv[1] = mv[1] / s(1) * 255.;
//						mv[2] = mv[2] / s(2) * 255.;
//						merge(mv,src);
//					}
//					imshow("color normed",src);
//				}
//				// ============================== //
//				
//				Mat im = loadGT(f,train_basename);
//				resize(im,im,src.size(),scale,scale, INTER_NEAREST);
//				imshow("im",im);
//				
//				
//				if(VISUALIZE){
//					Mat dsp;
//					src.copyTo(dsp,im);
//					imshow("ground truth",dsp);
//				}
//				
//				vector<KeyPoint> keypts;
//				Mat desc;
//				
//				dfd.detect(src,keypts);
//				
//				appendFeatures(src,keypts,desc);
//				
//				Mat fx;
//				appendDiffStackedResponse(src.size(),keypts,desc,METHOD,k,fx);
//				
//				subsampleDescriptors(keypts,desc,NUM_STACK*SAMPLE_STEP,k,fx);						// every layer trained on different pixels			
//				
//				Mat lb = getLabels(keypts,im);
//				
//				if(!dats.data) desc.copyTo(dats);
//				else dats.push_back(desc);
//				if(!labs.data) lb.copyTo(labs);
//				else labs.push_back(lb);
//				if(!f_x.data) fx.copyTo(f_x);
//				else f_x.push_back(fx);
//				
//				cout << f << "] data:" << dats.rows << " skin:" << countNonZero(labs) << " dims:" << dats.cols << endl;
//				
//			} // while loop (video)
//			
//			//==== kmeans ==== //
//			if(0){
//				gif.convertTo(gif,CV_32FC1,1/255.,0);
//				cout << gif << endl;
//				
//				int K = 3;
//				Mat kmeans_labs;
//				int attempts = 10;
//				Mat centers;
//				TermCriteria crit(TermCriteria::MAX_ITER +TermCriteria::EPS,30,0.01);
//				kmeans(gif,K,kmeans_labs,crit,attempts,KMEANS_PP_CENTERS,centers);
//				cout << "kmeans labs:" << kmeans_labs.t() << endl;
//				cout << "kmeans centers:" << centers << endl;
//			}
//			//================ //
//			
//			cout << "Data dims: " << dats.cols << endl;
//			if(VISUALIZE) destroyAllWindows();		
//			
//			Mat p_x;
//			Mat exp_fx;
//			exp(sigmoid_factor*f_x,exp_fx);
//			p_x = exp_fx / (1 + exp_fx);		// initially all 0.5
//			
//			Mat loss = labs - p_x;
//			
//			stringstream modelfile;
//			modelfile << root << "models/" << train_basename << model_code << "_" << k;
//			trainModel(dats,loss,modelfile.str(),METHOD);
//			
//			
//			//========================================================//
//			
//			Mat bin;
//			threshold(p_x,bin,0.49,1.0,THRESH_BINARY);
//			Mat zeroOneLoss = labs - bin;
//			
//			// compute precision and recall
//			Mat pos;
//			bin.convertTo(pos,CV_8UC1);			// all positive responses
//			Mat tp;
//			labs.copyTo(tp,pos);				// all true positives in positive responses
//			
//			cout << "detected positives:" <<  sum(pos)(0) << endl;
//			cout << "detected true positives:" <<  sum(tp)(0) << endl;
//			cout << "all true positives:" <<  sum(labs)(0) << endl;
//			
//			float precision = sum(tp)(0) / sum(pos)(0);
//			float recall = sum(tp)(0) / sum(labs)(0);
//			float fmeasure = 2.0*precision*recall / (precision+recall);
//			
//			cout << "==========================" << endl;
//			cout << "Zero One Loss: " << sum(abs(zeroOneLoss))(0) << endl;
//			cout << "Precision: " << precision << endl;
//			cout << "Recall: " << recall << endl;
//			cout << "Fmeasure: " << fmeasure << endl;
//			
//			fileout << k;
//			fileout << "\t" << sum(abs(zeroOneLoss))(0);
//			fileout << "\t" << precision;
//			fileout << "\t" << recall;
//			fileout << "\t" << fmeasure;
//			fileout << endl;
//			
//		} // for each layer ...
//	}
	
}
